#ifndef _VILENS_LIDAR_PREPROCESSOR_H_
#define _VILENS_LIDAR_PREPROCESSOR_H_

#include "lci_slam/vilens/common_header.h"

#define IS_VALID(a) ((abs(a) > 1e8) ? true : false)

namespace velodyne_ros {
struct EIGEN_ALIGN16 Point {
    PCL_ADD_POINT4D;
    float intensity;
    float time;
    uint16_t ring;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}  // namespace velodyne_ros
POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,
                                  (float, x, x)(float, y, y)(float, z, z)(
                                      float, intensity,
                                      intensity)(float, time, time)(uint16_t,
                                                                    ring, ring))

namespace ouster_ros {
struct EIGEN_ALIGN16 Point {
    PCL_ADD_POINT4D;
    float intensity;
    uint32_t t;
    uint16_t reflectivity;
    uint8_t ring;
    uint16_t ambient;
    uint32_t range;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}  // namespace ouster_ros
POINT_CLOUD_REGISTER_POINT_STRUCT(
    ouster_ros::Point,
    (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)
    // use std::uint32_t to avoid conflicting with pcl::uint32_t
    (std::uint32_t, t, t)(std::uint16_t, reflectivity, reflectivity)(
        std::uint8_t, ring, ring)(std::uint16_t, ambient,
                                  ambient)(std::uint32_t, range, range))

namespace vilens {

enum LID_TYPE { AVIA = 1, VELO16, OUST64, GENERAL };  //{1, 2, 3, 4}
enum Feature {
    Nor,
    Poss_Plane,
    Real_Plane,
    Edge_Jump,
    Edge_Plane,
    Wire,
    ZeroPoint
};
enum Surround { Prev, Next };
enum E_jump { Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind };

struct orgtype {
    double range;
    double dista;
    double angle[2];
    double intersect;
    E_jump edj[2];
    Feature ftype;
    orgtype() {
        range = 0;
        edj[Prev] = Nr_nor;
        edj[Next] = Nr_nor;
        ftype = Nor;
        intersect = 2;
    }
};

class lidarPreprocessor {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef std::shared_ptr<lidarPreprocessor> Ptr;

    lidarPreprocessor();
    ~lidarPreprocessor();

    void process(const lci_slam::CustomMsg::ConstPtr &msg,
                 PointCloudXYZI::Ptr &pcl_out);
    void process(const sensor_msgs::PointCloud2::ConstPtr &msg,
                 PointCloudXYZI::Ptr &pcl_out);
    void set_config(bool feat_en, int lid_type, double bld, int pfilt_num);

    // sensor_msgs::PointCloud2::ConstPtr pointcloud;
    PointCloudXYZI pl_full, pl_corn, pl_surf;
    PointCloudXYZI pl_buff[128];  // maximum 128 line lidar
    vector<orgtype> typess[128];  // maximum 128 line lidar
    int lidar_type, point_filter_num, N_SCANS, SCAN_RATE;
    double blind;
    bool feature_enabled, given_offset_time;
    ros::Publisher pub_full, pub_surf, pub_corn;

private:
    void avia_handler(const lci_slam::CustomMsg::ConstPtr &msg);
    void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
    void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
    void general_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
    void give_feature(PointCloudXYZI &pl, vector<orgtype> &types);
    void pub_func(PointCloudXYZI &pl, const ros::Time &ct);
    int plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i,
                    uint &i_nex, Eigen::Vector3d &curr_direct);
    bool small_plane(const PointCloudXYZI &pl, vector<orgtype> &types,
                     uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct);
    bool edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &types,
                         uint i, Surround nor_dir);

    int group_size;
    double disA, disB, inf_bound;
    double limit_maxmid, limit_midmin, limit_maxmin;
    double p2l_ratio;
    double jump_up_limit, jump_down_limit;
    double cos160;
    double edgea, edgeb;
    double smallp_intersect, smallp_ratio;
    double vx, vy, vz;
};

}  // namespace vilens

#endif